﻿/**
* @file mediatype.c in InsCube
* @author rebeater
* @comment
* Create on 2/19/22 2:05 PM
* @version 1.0
**/
#include "mediatype.h"
#include <memory.h>
#include "rtcommon.h"
#if IMU_DEVICE == IMU_ADIS16460 || IMU_DEVICE == IMU_ADIS16465
/**
 * ADIS1646X原始burstread数据转换为ImuData结构体
 * @param adi 原始数据
 * @param imu 输出结构体
 */
void convert_adi_to_double(ImuRawAdi *adi, ImuData *imu) {
  imu->gyro[0] = adis16460_kg * adi->gyro[0];
  imu->gyro[1] = adis16460_kg * adi->gyro[1];
  imu->gyro[2] = adis16460_kg * adi->gyro[2];

  imu->acce[0] = adis16460_ka_g * adi->acce[0];
  imu->acce[1] = adis16460_ka_g * adi->acce[1];
  imu->acce[2] = adis16460_ka_g * adi->acce[2];
}
#endif
/**
 * 转换里程计速度原始数据到Vel结构体
 * @param raw 原始数据
 * @param vel 输出结构体
 * @return 0
 */
int ConvertVelRawToFloat(VelocityRaw *raw, Velocity *vel) {
  vel->forward = ((float)((int16_t)(raw->vh_ << 8u) | raw->vl_)) / 1000.0f;
  vel->angular = ((float)((int16_t)(raw->ah_ << 8u) | raw->al_)) / 1000.0f;
  return 0;
}
/**
 * CRC编码输出 位置姿态和原始IMU数据
 * @param buffer
 * @param gps_week  GPS周
 * @param gpst GPS周内秒
 * @param nav 位置速度姿态
 * @param imu IMU原始数据
 * @return 有效的buffer长度
 */
int OutputEncode(uint8_t buffer[], short gps_week, double gpst, NavOutput *nav, IMU_RAW *imu) {
  int pva_sz = sizeof(NavOutput);
  int imu_sz = sizeof(IMU_RAW);
  buffer[0] = 0xAA;
  buffer[1] = 0x55;/*帧*/
  memcpy(buffer + 2, &gps_week, 4);
  memcpy(buffer + 6, &gpst, 8);
  memcpy(buffer + 14, nav, pva_sz);
  memcpy(buffer + pva_sz+14,imu,imu_sz);
  uint32_t crc = crc_checksum(&buffer[0], pva_sz+imu_sz + 14);
  buffer[pva_sz+imu_sz + 14] = (crc >> 24) & 0xFF;
  buffer[pva_sz+imu_sz + 15] = (crc >> 16) & 0xFF;
  buffer[pva_sz+imu_sz + 16] = (crc >> 8) & 0xFF;
  buffer[pva_sz+imu_sz + 17] = (crc) & 0xFF;
  return pva_sz + imu_sz + 18;
}